/*
-----------------------------------------------------------------------------
This source file is part of the OGRE Reference Application, a layer built
on top of OGRE(Object-oriented Graphics Rendering Engine)
For the latest info, see http://www.ogre3d.org/

Copyright (c) 2000-2006 Torus Knot Software Ltd
Also see acknowledgements in Readme.html

This program is free software; you can redistribute it and/or modify it under
the terms of the GNU Lesser General Public License as published by the Free Software
Foundation; either version 2 of the License, or (at your option) any later
version.

This program is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public License along with
this program; if not, write to the Free Software Foundation, Inc., 59 Temple
Place - Suite 330, Boston, MA 02111-1307, USA, or go to
http://www.gnu.org/copyleft/lesser.txt.

You may alternatively use this source under the terms of a specific version of
the OGRE Unrestricted License provided you have obtained such a license from
Torus Knot Software Ltd.
-----------------------------------------------------------------------------
*/
#include "OgreRefAppSoccerTeam.h"
#include "OgreRefAppFieldPlayer.h"
#include "OgreRefAppFieldPlayerOwnedStates.h"
#include "OgreRefAppGoalKeeper.h"
#include "OgreRefAppGoalKeeperOwnedStates.h"
#include "OgreRefAppBall.h"
#include "OgreRefAppSoccerPitch.h"
#include "OgreRefAppWorld.h"
#include "OgreRefAppGoal.h"
#include "OgreRefAppUtils.h"

#define __DEBUGON__

namespace OgreRefApp
{

    SoccerTeam::SoccerTeam(Goal*        home_goal,
                           Goal*        opponents_goal,
                           SoccerPitch* pitch,
                           team_color   color):
            m_pOpponentsGoal(opponents_goal),
            m_pHomeGoal(home_goal),
            m_pOpponents(NULL),
            m_pPitch(pitch),
            m_Color(color),
            m_dDistSqToBallOfClosestPlayer(0.0),
            m_pSupportingPlayer(NULL),
            m_pReceivingPlayer(NULL),
            m_pControllingPlayer(NULL),
            m_pPlayerClosestToBall(NULL)
    {

        //setup the state machine
        m_pStateMachine = new StateMachine<SoccerTeam>(this);

        m_pStateMachine->SetCurrentState(Defending::Instance());
        m_pStateMachine->SetPreviousState(Defending::Instance());
        m_pStateMachine->SetGlobalState(NULL);



        //create the players and goalkeeper
        CreatePlayers();

        //set default steering behaviors
        std::vector<PlayerBase*>::iterator it = m_Players.begin();

        for (it; it != m_Players.end(); ++it)
        {
            (*it)->getSteeringBehavior()->SeparationOn();
        }


        m_pStateMachine->CurrentState()->Enter(this);

        //create the sweet spot calculator
        m_pSupportSpotCalc = new SupportSpotCalculator(NumSweetSpotsX,
                                                 NumSweetSpotsY,
                                                 this);

    }

    SoccerTeam::~SoccerTeam()
    {

    }

    void SoccerTeam::CreatePlayers()
    {

        if (Color() == blue)
        {
            //create the players


            GoalKeeper* goalKeeper = World::getSingletonPtr()->createGoalKeeper("goalKeeperBlue5", 7,PlayerBase::goal_keeper, TendGoal::Instance(), this, 1, Vector3(0*40 + 10,4*40 + 10,7) );

            goalKeeper->setDynamicsEnabled(true);
            goalKeeper->getEntity()->setMaterialName("Examples/Hilite/Purple");
            m_Players.push_back(goalKeeper);


            FieldPlayer* fieldPlayer = World::getSingletonPtr()->createFieldPlayer("fieldPlayerBlue1", 7, PlayerBase::defender, Wait::Instance(), this, 3, Vector3(3*40 + 10,2*40 + 10,7));

            fieldPlayer->setDynamicsEnabled(true);
            fieldPlayer->getEntity()->setMaterialName("Examples/Hilite/Blue");
            m_Players.push_back(fieldPlayer);

            fieldPlayer = World::getSingletonPtr()->createFieldPlayer("fieldPlayerBlue2", 7,PlayerBase::defender, Wait::Instance(), this, 5, Vector3(3*40 + 10,7*40 + 10,7));

            fieldPlayer->setDynamicsEnabled(true);
            fieldPlayer->getEntity()->setMaterialName("Examples/Hilite/Blue");
            m_Players.push_back(fieldPlayer);

            fieldPlayer = World::getSingletonPtr()->createFieldPlayer("fieldPlayerBlue3", 7,PlayerBase::attacker, Wait::Instance(), this, 6, Vector3(7*40 + 10,2*40 + 10,7) );

            fieldPlayer->setDynamicsEnabled(true);

            fieldPlayer->getEntity()->setMaterialName("Examples/Hilite/Blue");
            m_Players.push_back(fieldPlayer);

            fieldPlayer = World::getSingletonPtr()->createFieldPlayer("fieldPlayerBlue4", 7,PlayerBase::attacker, Wait::Instance(), this, 8, Vector3(7*40 + 10,7*40 + 10,7) );

            fieldPlayer->setDynamicsEnabled(true);
            fieldPlayer->getEntity()->setMaterialName("Examples/Hilite/Blue");
            m_Players.push_back(fieldPlayer);



        }
        else
        {


            //create the players


            GoalKeeper* goalKeeper = World::getSingletonPtr()->createGoalKeeper("goalKeeperRed5", 7,PlayerBase::goal_keeper,TendGoal::Instance(), this, 16, Vector3(19*40 + 10,4*40 + 10,15) );

            goalKeeper->setDynamicsEnabled(true);
            goalKeeper->getEntity()->setMaterialName("Examples/Hilite/Yellow");
            m_Players.push_back(goalKeeper);


            FieldPlayer* fieldPlayer = World::getSingletonPtr()->createFieldPlayer("fieldPlayerRed1", 7,PlayerBase::defender, Wait::Instance(),this, 12, Vector3(16*40 + 10,2*40 + 10,25) );
            fieldPlayer->setDynamicsEnabled(true);
            fieldPlayer->getEntity()->setMaterialName("Examples/Hilite/Red");
            m_Players.push_back(fieldPlayer);
            /*
            #ifdef __DEBUGON__
            std::cout << std::endl<< fieldPlayer->HomeRegion()->Center() << std::endl;
            #endif
            */
            fieldPlayer = World::getSingletonPtr()->createFieldPlayer("fieldPlayerRed2", 7,PlayerBase::defender, Wait::Instance(), this, 14, Vector3(16*40 + 10,7*40 + 10,35) );

            fieldPlayer->setDynamicsEnabled(true);
            fieldPlayer->getEntity()->setMaterialName("Examples/Hilite/Red");
            m_Players.push_back(fieldPlayer);

            fieldPlayer = World::getSingletonPtr()->createFieldPlayer("fieldPlayerRed3", 7,PlayerBase::attacker, Wait::Instance(), this, 9, Vector3(12*40 + 10,2*40 + 10,45) );

            fieldPlayer->setDynamicsEnabled(true);
            fieldPlayer->getEntity()->setMaterialName("Examples/Hilite/Red");
            m_Players.push_back(fieldPlayer);

            fieldPlayer = World::getSingletonPtr()->createFieldPlayer("fieldPlayerRed4", 7,PlayerBase::attacker, Wait::Instance(), this, 11, Vector3(12*40 + 10,7*40 + 10,55) );

            fieldPlayer->setDynamicsEnabled(true);
            fieldPlayer->getEntity()->setMaterialName("Examples/Hilite/Red");
            m_Players.push_back(fieldPlayer);






        }


    }

    void SoccerTeam::update()
    {

        CalculateClosestPlayerToBall();


        m_pStateMachine->Update();



        //now update each player
        std::vector<PlayerBase*>::iterator it = m_Players.begin();

        for (it; it != m_Players.end(); ++it)
        {
            (*it)->update();

        }





    }

    void SoccerTeam::CalculateClosestPlayerToBall()
    {

        Real ClosestSoFar = MaxFloat;

        std::vector<PlayerBase*>::iterator it = m_Players.begin();

        for (it; it != m_Players.end(); ++it)
        {
            //calculate the dist. Use the squared value to avoid sqrt
            Real dist = (*it)->getPosition().squaredDistance( m_pPitch->ball()->getPosition() );

            //keep a record of this value for each player
            (*it)->SetDistSqToBall(dist);

            if (dist < ClosestSoFar)
            {
                ClosestSoFar = dist;
                if ( m_pPlayerClosestToBall != NULL )
                    m_pPlayerClosestToBall->getSteeringBehavior()->PursuitOff();
                m_pPlayerClosestToBall = (*it);
                m_pPlayerClosestToBall->getSteeringBehavior()->PursuitOn(Pitch()->ball());
            }
        }

        m_dDistSqToBallOfClosestPlayer = ClosestSoFar;

    }



//------------- DetermineBestSupportingAttacker ------------------------
//
// calculate the closest player to the SupportSpot
//------------------------------------------------------------------------
    PlayerBase* SoccerTeam::DetermineBestSupportingAttacker()
    {
        double ClosestSoFar = MaxFloat;

        PlayerBase* BestPlayer = NULL;

        std::vector<PlayerBase*>::iterator it = m_Players.begin();

        for (it; it != m_Players.end(); ++it)
        {


            //only attackers utilize the BestSupportingSpot
            if ( ((*it)->Role() == PlayerBase::attacker) && ((*it) != m_pControllingPlayer) )
            {

                //calculate the dist. Use the squared value to avoid sqrt
                (*it)->Team()->SetSupportingPlayer((*it));
                double dist = (*it)->getPosition().squaredDistance( m_pSupportSpotCalc->GetBestSupportingSpot());
                return NULL;
                //if the distance is the closest so far and the player is not a
                //goalkeeper and the player is not the one currently controlling
                //the ball, keep a record of this player
                if ((dist < ClosestSoFar) )
                {
                    ClosestSoFar = dist;
                    return (*it);
                    BestPlayer = (*it);

                }

            }

        }


        return BestPlayer;
    }

//-------------------------- FindPass ------------------------------
//
//  The best pass is considered to be the pass that cannot be intercepted
//  by an opponent and that is as far forward of the receiver as possible
//------------------------------------------------------------------------
    bool SoccerTeam::FindPass(PlayerBase* passer,
                              PlayerBase*           receiver,
                              Ogre::Vector3              PassTarget,
                              Real                  power,
                              Real                  MinPassingDistance)
    {

        std::vector<PlayerBase*>::const_iterator curPlyr = Members().begin();

        double    ClosestToGoalSoFar = MaxFloat;
        Ogre::Vector3 Target;

        //iterate through all this player's team members and calculate which
        //one is in a position to be passed the ball
        for (curPlyr; curPlyr != Members().end(); ++curPlyr)
        {
            std::cout << "findPass for "<< std::endl;

            //make sure the potential receiver being examined is not this player
            //and that it is further away than the minimum pass distance
            if ( (*curPlyr != passer) &&
                    (passer->getPosition().squaredDistance((*curPlyr)->getPosition()) >
                     MinPassingDistance*MinPassingDistance))
            {

                //if (GetBestPassToReceiver(passer, *curPlyr, Target, power))
                //{
                    Target = Ogre::Vector3( (*curPlyr)->getPosition().x, (*curPlyr)->getPosition().y, (*curPlyr)->getPosition().z );
                    std::cout << "tem gente pra tocar ainda"<< std::endl;
                    //if the pass target is the closest to the opponent's goal line found
                    // so far, keep a record of it
                    double Dist2Goal = fabs(Target.x - OpponentsGoal()->Center().x);

                    if (Dist2Goal < ClosestToGoalSoFar)
                    {

                        ClosestToGoalSoFar = Dist2Goal;

                        //keep a record of this player
                        receiver = (*curPlyr);
                        std::cout << "achou recebedor"<< receiver->getPosition() << std::endl;
                        passer->Team()->SetReceiver(receiver);
                        //and the target
                        PassTarget = Target;

                    }

                //}

            }

        }//next team member


        if (receiver) return true;

        else return false;
    }


//---------------------- GetBestPassToReceiver ---------------------------
//
//  Three potential passes are calculated. One directly toward the receiver's
//  current position and two that are the tangents from the ball position
//  to the circle of radius 'range' from the receiver.
//  These passes are then tested to see if they can be intercepted by an
//  opponent and to make sure they terminate within the playing area. If
//  all the passes are invalidated the function returns false. Otherwise
//  the function returns the pass that takes the ball closest to the
//  opponent's goal area.
//------------------------------------------------------------------------
    bool SoccerTeam::GetBestPassToReceiver(PlayerBase* passer,
                                           PlayerBase* receiver,
                                           Ogre::Vector3               PassTarget,
                                           Real                  power )
    {
        //first, calculate how much time it will take for the ball to reach
        //this receiver, if the receiver was to remain motionless
        Real time = Pitch()->ball()->TimeToCoverDistance(Pitch()->ball()->getPosition(),
                      receiver->getPosition(),
                      power);

        //return false if ball cannot reach the receiver after having been
        //kicked with the given power
        if (time < 0) return false;

        //the maximum distance the receiver can cover in this time
         Real InterceptRange = time * receiver->getMaxLinearVelocity().length();

        //Scale the intercept range
        const double ScalingFactor = 0.3;
        InterceptRange *= ScalingFactor;

        //now calculate the pass targets which are positioned at the intercepts
        //of the tangents from the ball to the receiver's range circle.
        Ogre::Vector3 ip1, ip2;

        GetTangentPoints(receiver->getPosition(),
                         InterceptRange,
                         Pitch()->ball()->getPosition(),
                         ip1,
                         ip2);

        const int NumPassesToTry = 3;
        Ogre::Vector3 Passes[NumPassesToTry] = {ip1, receiver->getPosition(), ip2};


        // this pass is the best found so far if it is:
        //
        //  1. Further upfield than the closest valid pass for this receiver
        //     found so far
        //  2. Within the playing area
        //  3. Cannot be intercepted by any opponents

        double ClosestSoFar = MaxFloat;
        bool  bResult      = false;

        for (int pass=0; pass<NumPassesToTry; ++pass)
        {
            double dist = fabs(Passes[pass].x - OpponentsGoal()->Center().x);

            if (( dist < ClosestSoFar) &&
                    Pitch()->playingArea()->Inside(Ogre::Vector2( Passes[pass].x, Passes[pass].y )) &&
                    isPassSafeFromAllOpponents(Pitch()->ball()->getPosition(),
                                               Passes[pass],
                                               receiver,
                                               power))

            {
                ClosestSoFar = dist;
                PassTarget   = Passes[pass];
                bResult      = true;
            }
        }

        return bResult;
    }

//----------------------- isPassSafeFromOpponent -------------------------
//
//  test if a pass from 'from' to 'to' can be intercepted by an opposing
//  player
//------------------------------------------------------------------------
    bool SoccerTeam::isPassSafeFromOpponent(Ogre::Vector3    from,
                                            Ogre::Vector3    target,
                                            PlayerBase* receiver,
                                            PlayerBase* opp,
                                            Real       PassingForce)
    {
        //move the opponent into local space.
        Ogre::Vector3 ToTarget = target - from;
        Ogre::Vector3 ToTargetNormalized = ToTarget / ToTarget.normalise();
        /*
        Ogre::Vector3 LocalPosOpp = PointToLocalSpace(opp->getPosition(),
                               ToTargetNormalized,
                               ToTargetNormalized.perpendicular(),
                               from);

        //if opponent is behind the kicker then pass is considered okay(this is
        //based on the assumption that the ball is going to be kicked with a
        //velocity greater than the opponent's max velocity)
        if ( LocalPosOpp.x < 0 )
        {
            return true;
        }
        */
        //if the opponent is further away than the target we need to consider if
        //the opponent can reach the position before the receiver.
        if (from.squaredDistance( target ) < opp->getPosition().squaredDistance( from ))
        {
            if (receiver)
            {
                if ( target.squaredDistance( opp->getPosition() )  >
                        target.squaredDistance( receiver->getPosition() ) )
                {
                    return true;
                }

                else
                {
                    return false;
                }

            }

            else
            {
                return true;
            }
        }

        //calculate how long it takes the ball to cover the distance to the
        //position orthogonal to the opponents position
        /*
        Real TimeForBall =
            Pitch()->ball()->TimeToCoverDistance(Ogre::Vector3(0,0,0),
                                                 Ogre::Vector3(LocalPosOpp.x, 0,0),
                                                 PassingForce);
        */

        Real TimeForBall =
            Pitch()->ball()->TimeToCoverDistance(Ogre::Vector3(0,0,0),
                                                 Ogre::Vector3(ToTarget.x, 0,0),
                                                 PassingForce);
        //now calculate how far the opponent can run in this time
        Real reach = opp->getLinearVelocity().length() * TimeForBall +
                       BallRadius+
                       PlayerHeight/2;



        //if the distance to the opponent's y position is less than his running
        //range plus the radius of the ball and the opponents radius then the
        //ball can be intercepted
        /*
        if ( fabs(LocalPosOpp.y) < reach )
        {
            return false;
        }
        */

        if ( fabs(ToTarget.y) < reach )
        {
            return false;
        }

        return true;
    }

//---------------------- isPassSafeFromAllOpponents ----------------------
//
//  tests a pass from position 'from' to position 'target' against each member
//  of the opposing team. Returns true if the pass can be made without
//  getting intercepted
//------------------------------------------------------------------------
    bool SoccerTeam::isPassSafeFromAllOpponents(Ogre::Vector3                from,
            Ogre::Vector3                target,
            PlayerBase* receiver,
            Real     PassingForce)
    {
        std::vector<PlayerBase*>::const_iterator opp = Opponents()->Members().begin();

        for (opp; opp != Opponents()->Members().end(); ++opp)
        {
            if (!isPassSafeFromOpponent(from, target, receiver, *opp, PassingForce))
            {

                return false;
            }
        }

        return true;
    }

//------------------------ CanShoot --------------------------------------
//
//  Given a ball position, a kicking power and a reference to a vector2D
//  this function will sample random positions along the opponent's goal-
//  mouth and check to see if a goal can be scored if the ball was to be
//  kicked in that direction with the given power. If a possible shot is
//  found, the function will immediately return true, with the target
//  position stored in the vector ShotTarget.
//------------------------------------------------------------------------
    bool SoccerTeam::CanShoot(Ogre::Vector3  BallPos,
                              Real     power,
                              Ogre::Vector3* ShotTarget)
    {
        //the number of randomly created shot targets this method will test
        int NumAttempts = NumAttemptsToFindValidStrike;

        while (NumAttempts--)
        {
            //choose a random position along the opponent's goal mouth. (making
            //sure the ball's radius is taken into account)
            ShotTarget = new Ogre::Vector3( OpponentsGoal()->Center() );

            //the y value of the shot position should lay somewhere between two
            //goalposts (taking into consideration the ball diameter)
            int MinYVal = OpponentsGoal()->LeftPost().y + BallRadius;
            int MaxYVal = OpponentsGoal()->RightPost().y - BallRadius;

            ShotTarget->y = (double)RandInt(MinYVal, MaxYVal);

            //make sure striking the ball with the given power is enough to drive
            //the ball over the goal line.
            double time = Pitch()->ball()->TimeToCoverDistance(BallPos,
                          Ogre::Vector3(ShotTarget->x, ShotTarget->y, ShotTarget->z),
                          power);

            //if it is, this shot is then tested to see if any of the opponents
            //can intercept it.
            //if (time >= 0)
            //{
                if (isPassSafeFromAllOpponents(BallPos, Ogre::Vector3(ShotTarget->x, ShotTarget->y, ShotTarget->z), NULL, power))
                {
                    return true;
                }
            //}
        }

        return false;
    }


//--------------------- ReturnAllFieldPlayersToHome ---------------------------
//
//  sends a message to all players to return to their home areas forthwith
//------------------------------------------------------------------------
    void SoccerTeam::ReturnAllFieldPlayersToHome()
    {
        std::vector<PlayerBase*>::const_iterator it = m_Players.begin();

        for (it; it != m_Players.end(); ++it)
        {
            if ((*it)->Role() != PlayerBase::goal_keeper)
            {

                ApplicationObject::CollisionInfo collInfo;
                collInfo.Msg = Msg_GoHome;

                #ifdef __DEBUGON__
                std::cout << "vai enviar" << std::endl;
                #endif


                (*it)->handleMessage( collInfo );

                #ifdef __DEBUGON__
                std::cout << "enviou" << std::endl;
                #endif
            }
        }
    }

/*
    PlayerBase* SoccerTeam::GetPlayerFromID(int id)const
    {
        std::vector<PlayerBase*>::const_iterator it = m_Players.begin();

        for (it; it != m_Players.end(); ++it)
        {
            if ((*it)->ID() == id) return *it;
        }

        return NULL;
    }
*/

    void SoccerTeam::SetPlayerHomeRegion(int plyr, int region)
    {


        m_Players[plyr]->SetHomeRegion(region);
    }


//---------------------- UpdateTargetsOfWaitingPlayers ------------------------
//
//
    void SoccerTeam::UpdateTargetsOfWaitingPlayers()
    {
        std::vector<PlayerBase*>::const_iterator it = m_Players.begin();

        for (it; it != m_Players.end(); ++it)
        {

            if ( (*it)->Role() != PlayerBase::goal_keeper )
            {

                //cast to a field player
                FieldPlayer* plyr = static_cast<FieldPlayer*>(*it);

                if ( plyr->GetFSM()->isInState(Wait::Instance()) ||
                        plyr->GetFSM()->isInState(ReturnToHomeRegion::Instance()) )
                {
                    plyr->Steering()->SetTarget(Ogre::Vector3( plyr->HomeRegion()->Center().x, plyr->HomeRegion()->Center().y, BallRadius));
                }


            }


        }


    }


//--------------------------- AllPlayersAtHome --------------------------------
//
//  returns false if any of the team are not located within their home region
//-----------------------------------------------------------------------------
    bool SoccerTeam::AllPlayersAtHome()
    {
        std::vector<PlayerBase*>::const_iterator it = m_Players.begin();

        for (it; it != m_Players.end(); ++it)
        {
            if ((*it)->InHomeRegion() == false)
            {
                return false;
            }
        }

        return true;
    }

//------------------------- RequestPass ---------------------------------------
//
//  this tests to see if a pass is possible between the requester and
//  the controlling player. If it is possible a message is sent to the
//  controlling player to pass the ball asap.
//-----------------------------------------------------------------------------
    void SoccerTeam::RequestPass(PlayerBase* requester)
    {
        //maybe put a restriction here
        if (RandFloat() > 0.1) return;

        if (isPassSafeFromAllOpponents(ControllingPlayer()->getPosition(),
                                       requester->getPosition(),
                                       requester,
                                       MaxPassingForce))
        {

            //tell the player to make the pass
            //let the receiver know a pass is coming

            ApplicationObject::CollisionInfo collInfo;
            collInfo.Msg = Msg_PassToMe;
            collInfo.sender = requester;


            ControllingPlayer()->handleMessage( collInfo );

        }
    }


//----------------------------- isOpponentWithinRadius ------------------------
//
//  returns true if an opposing player is within the radius of the position
//  given as a parameter
//-----------------------------------------------------------------------------
    bool SoccerTeam::isOpponentWithinRadius(Ogre::Vector3 pos, Real rad)
    {
        std::vector<PlayerBase*>::const_iterator end = Opponents()->Members().end();
        std::vector<PlayerBase*>::const_iterator it;

        for (it=Opponents()->Members().begin(); it !=end; ++it)
        {
            if (pos.squaredDistance( (*it)->getPosition() ) < rad*rad)
            {
                return true;
            }
        }

        return false;
    }



}
